from pi.controller.moveController import moveController
from pi.controller.sensorController import sensorController
from pi.controller.ultrasonicServoController import ultrasonicServoController
# from pi.controller.buzzerController import buzzerController
from pi.controller.infraredController import infraredController
from pi.model.instruction import instruction
from pi.model.packet import packet
from pi.model.data import data
from pi.config import *
import socket
import time
import threading
import random

"""
    这是小车运行的主程序，直接运行即可
"""


def service_thread(_sock: socket.socket):
    # car = moveController(trace_stack=trace_stack)
    car = con_move
    # threading.Thread(target=avoid_run, args=(car, ultrasonicServoController, infraredController,)).start()  # 执行线程体
    threading.Thread(target=avoid_run, args=()).start()  # 执行避障线程
    while True:
        try:
            _recv = _sock.recv(1024)
            if _recv == b'':  # socket断开
                car.run(instruction(ins_type=instruction.DISCONECT_STOP))  # 停止
                return
            else:
                """解析报文"""
                mypacket = packet()
                mypacket.from_bin(_recv)  # 从报文获取数据填充到packet对象内
                """判断pac_type"""
                if mypacket.pac_type != mypacket.PC_TO_PI:  # 检查ctltype为pc to pi
                    raise ValueError("ctl_type出错", mypacket.pac_type)
                """判断ctl_type"""
                if mypacket.ctl_type == packet.CAR_MOVE:  # 小车移动
                    car.run(mypacket.get_instruction())
                elif mypacket.ctl_type == packet.CAR_STOP:  # 小车停止
                    car.stop()  # 停止
                elif mypacket.ctl_type == packet.BACK_HOME:  # 回家
                    car.run_back_home()
                elif mypacket.ctl_type == packet.SENSOR_UP:  # 开传感器
                    # car.sensor_up(mypacket.get_instruction().sensor_list)
                    pass
                elif mypacket.ctl_type == packet.SENSOR_DOWN:  # 关传感器
                    # car.sensor_down(mypacket.get_instruction().sensor_list)
                    pass
                elif mypacket.ctl_type == packet.ROUTER:  # 转发
                    pass
                elif mypacket.ctl_type == packet.INIT_CONF:  # 初始化 / 修改配置信息
                    # todo：调用函数，修改配置文件即可（传输数据结构未定）
                    pass
                else:
                    raise ValueError("pac_type出错", mypacket.ctl_type)

        except KeyboardInterrupt:
            car.run(instruction(ins_type=instruction.SIG_INIT_STOP))  # 停止
            sock.close()


def send_back_data(_sock: socket.socket, time_interval: int = 3):
    """
    每隔time_interval时间就发送一次数据
    :param time_interval: 获取数据以及发送报文的时间间隔
    :param _sock:
    :return:
    """
    sensorCon = sensorController()  # todo：传感器列表从哪来 ？
    sensor_data = data()  # 建一个空的data对象
    sensor_data.set_type(data.DATA_TRACE_AND_TRACE)  # 设置为 传感器类型
    while True:
        try:
            # sensor_data.set_data_list(sensorCon.get_data_all())  # 获取并填充数据
            sensor_data.set_data_list(sensorCon.get_data(sensorCon.SENSOR_LIG))  # 获取光照传感器数据
            # sensor_data.set_data_list(sensorCon.get_data(sensorCon.SENSOR_TEM))  # 获取温度传感器数据
        except:
            # print("读取传感器error！！！")
            # sensor_data.set_data_list(["error from cheney!!!", [111]])
            dataa = str(int(random.randint(1, 100)))
            sensor_data.set_data_list([dataa, [111]])

        # sensor_data.set_trace_stack(trace_stack)
        sensor_data.set_trace_stack(con_move.get_trace_stack())
        con_move.clear_trace_stack()  # 清空指令栈  todo：是否需要保留历史记录 ？发送失败怎么办 ？
        _mypac = packet(src_id="", des_id="",
                        pac_type=2, ctl_type=1,
                        content=sensor_data,
                        )  # 实例化一个报文对象，并填充数据
        ret = _sock.send(_mypac.to_bin())  # 对象转二进制再发送出去
        print(">>>>> 成功发送的位数：", ret)
        time.sleep(time_interval)  # 休息时间间隔


# def auto_run(move_con: moveController, sens_con: sensorController,
#              limit_l_r: int = 20, limit_front: int = 50):
#     """
#     避障程序
#     只要小车在运动，就检测障碍
#     :param limit_front: 前方距离小于该值 开始避障
#     :param limit_l_r: 左右距离小于该值 开始避障
#     :param sens_con:
#     :param move_con:
#     :return:
#     """
#     while True:
#         if not move_con.get_speed():  # 速度为0 不避障
#             continue
#         """
#             红外线检测，获得左右距离
#             超声波检测，获得前端距离
#             判断距离是否合法
#             然后进行避障碍
#             结束之后，从指令栈的拿出顶部的指令，赋给当前状态
#         """
#         dis_left, dis_head, dis_right = sens_con.get_distance_3()
#         if dis_left < limit_l_r and dis_right < limit_l_r and dis_head < limit_front:
#             continue
#         else:
#             print("开始避障!", dis_left, dis_head, dis_right)


# def avoid_run(move_con: moveController, ult_servo_con=ultrasonicServoController, infra_con=infraredController,
#                limit_l_r: int = 20, limit_front: int = 50, avoid_time: int = 1):
#     """
#     避障模块，前方有障碍就检测左右，向无障碍方向旋转规定的时间
#     :param avoid_time: 检测到障碍后，移动的时间
#     :param move_con: 控制移动
#     :param ult_servo_con: 控制舵机+超声波
#     :param infra_con: 控制红外线
#     :param buzz_con: 控制蜂鸣器
#     :param limit_l_r: 左右临界距离
#     :param limit_front: 前方临界距离
#     :return:
#     """
def avoid_run():
    _move_con = con_move
    _ult_servo_con = con_ult_servo
    _infra_con = con_infra
    limit_l_r = 20
    limit_front = 30
    avoid_time = 0.5  # 避障时运行的时间

    print("成功进入避障模块！！！")
    while True:
        close_l = _infra_con.left_closer()
        close_r = _infra_con.right_closer()
        if _ult_servo_con.scan_front() < limit_front:
            close_f = True  # 前方太近了
            print("前方距离过近！")
        else:
            close_f = False
            print("前方距离正常")

        if close_f:
            _move_con.stop()  # 先停下
            close_l = close_l or (_ult_servo_con.scan_left() < limit_l_r)
            close_r = close_r or (_ult_servo_con.scan_right() < limit_l_r)

        if close_l:
            print("》》》左方距离过近！")
        else:
            print("》》》左边正常")
        if close_l:
            print("《《《右方距离过近！")
        else:
            print("《《《右边正常")

        if close_l and close_r:  # 后退
            print("后退")
            _move_con.run(instruction(ins_type=instruction.CAR_MOVE, direction=[False, True, False, False]))
            time.sleep(avoid_time)
            _move_con.run(instruction(ins_type=instruction.DISCONECT_STOP))
            continue
        elif close_l:  # 右转
            print("右转")
            _move_con.run(instruction(ins_type=instruction.CAR_MOVE, direction=[False, False, False, True]))
            time.sleep(avoid_time)
            _move_con.run(instruction(ins_type=instruction.DISCONECT_STOP))
        elif close_r:  # 左转
            print("左转")
            _move_con.run(instruction(ins_type=instruction.CAR_MOVE, direction=[False, False, True, False]))
            time.sleep(avoid_time)
            _move_con.run(instruction(ins_type=instruction.DISCONECT_STOP))
        elif close_f:
            print("左转")
            _move_con.run(instruction(ins_type=instruction.CAR_MOVE, direction=[False, False, True, False]))
            time.sleep(avoid_time)
            _move_con.run(instruction(ins_type=instruction.DISCONECT_STOP))
        # else:
        #     _move_con.run(instruction(ins_type=instruction.CAR_MOVE, direction=[True, False, False, False]))


if __name__ == '__main__':
    while True:
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        address_s = (addr_BaseStation_1, port_BaseStation_1)
        # con_buzz = buzzerController()
        try:
            sock.connect(address_s)
        except ConnectionRefusedError:
            print("\nConnectionRefusedError!!!")
            for i in range(5):
                print("\rreconnecting after", 4 - i, "seconds", end="")
                time.sleep(1)
            continue
        print("成功连接到基站", sock)
        # con_buzz.on_network_connected()

        instruction_stack = []  # 指令栈，原来存储历史指令，每个元素都是instruction类型
        trace_stack = []  # 实际运行路径（实际运行的指令 = 服务器下发 + 避障）

        con_move = moveController(trace_stack=trace_stack)
        con_ult_servo = ultrasonicServoController()
        con_infra = infraredController()

        # 一个独立的线程发送传感器数据
        threading.Thread(target=send_back_data, args=(sock,)).start()

        # 一个独立的线程接收指令、控制小车
        carThread = threading.Thread(target=service_thread, args=(sock,))
        carThread.start()  # 执行线程体

        carThread.join()  # 将线程阻塞起来，线程退出之后才能继续向下执行，实现重连机制
        sock.close()
        # con_buzz.on_network_disconnected()
        print(">>> 断开连接，线程执行完毕\n>>> 正在重新连接... ...")
